#include "balance.h"

//Whether the robot model is incorrectly marked
//机器人型号是否错误标志位
int robot_mode_check_flag=0;
int A=1,B=1,C=1,DD=1; //Used for testing //用于测试
int Time_count=0; //Time variable //计时变量

#define N100  0
#define Jy901 1

/**************************************************************************
函数功能：增量式PI控制器
入口参数：编码器测量值(实际速度)，目标速度
返回  值：电机PWM
根据增量式离散PID公式 
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI_A (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
	 if(Bias/Target<0.06&&Bias/Target>-0.06)  Bias = 0;
	 Pwm+=MOTOR_A.Velocity_KP*(Bias-Last_bias)+MOTOR_A.Velocity_KI*Bias; 
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
	 return Pwm;    
}
int Incremental_PI_B (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
	 if(Bias/Target<0.06&&Bias/Target>-0.06)  Bias = 0;
	 Pwm+=MOTOR_B.Velocity_KP*(Bias-Last_bias)+MOTOR_B.Velocity_KI*Bias; 
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
	 return Pwm;    
}
int Incremental_PI_C (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
	 if(Bias/Target<0.06&&Bias/Target>-0.06)  Bias = 0;
	 Pwm+=MOTOR_C.Velocity_KP*(Bias-Last_bias)+MOTOR_C.Velocity_KI*Bias; 
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
	 return Pwm;    
}
int Incremental_PI_D (float Encoder,float Target)
{ 	
	 static float Bias,Pwm,Last_bias;
	 Bias=Target-Encoder; //Calculate the deviation //计算偏差
	 //滤波，速度越快需要值越小，0.1m/s时0.08效果较好，会减弱I的调控
	 //if(Bias/Target<0.1&&Bias/Target>-0.1)  Bias = 0;
	 if(Bias/Target<0.06&&Bias/Target>-0.06)  Bias = 0;
	 Pwm+=MOTOR_D.Velocity_KP*(Bias-Last_bias)+MOTOR_D.Velocity_KI*Bias; 
	 if(Pwm>7200)Pwm=7200;
	 if(Pwm<-7200)Pwm=-7200;
	 Last_bias=Bias; //Save the last deviation //保存上一次偏差 
	 return Pwm;    
}

/**************************************************************************
函数功能：由Target_z控制旋转速度
入口参数：无
返回值：Z轴速度
***************************************************************************/
float Vz_Control(void) //(float Angle_Z,float Target_AngleZ)
{ 	
	float Vz ,amplitude=1.5;
	//由目标角度与当前角度计算角度差
	Angle_Bias = Target_Z - Angle[2];	
	if(Angle_Bias>80) Turn_Flag = 1;
	if(Angle_Bias > 180)
	{
		Angle_Bias -= 360;
	} 
	else if(Angle_Bias < -180)
	{
		Angle_Bias += 360;
	}
	//角度差小于0.01时保持静止 
	//根据角度差决定Vz速度,角度差大于5快速旋转
	if(!Turn_Flag){
		if(float_abs(Angle_Bias)<=0.01) Vz = 0;
		else if(Angle_Bias>=30) Vz = amplitude/3;
		else if(Angle_Bias<=-30)Vz = amplitude/-3;
		else if(-30<Angle_Bias && Angle_Bias<=-3)
			Vz = amplitude/-20;
		else if(3<=Angle_Bias && Angle_Bias<30)
			Vz = amplitude/20;
		else if(-3<Angle_Bias && Angle_Bias<=-1)
			Vz = amplitude/-30;
		else if(1<=Angle_Bias && Angle_Bias<3)
			Vz = amplitude/30;
		else if(-1<Angle_Bias && Angle_Bias<=-0.01)
			Vz = amplitude/-40;
		else if(0.01<=Angle_Bias && Angle_Bias<1)
			Vz = amplitude/40;
	}
	else{
		if(float_abs(Angle_Bias)<=3) Turn_Flag = 0;
		else if(Angle_Bias>=3) Vz = amplitude/3;
		else if(Angle_Bias<=-3)Vz = amplitude/-3;		
	}
	return Vz;
}
/**************************************************************************
函数功能：由速度控制小车移动距离
入口参数：无
返回值：无
***************************************************************************/
void S_Control(float *Vx,float *Vy)
{
	u8 SX_Flag = 1,SY_Flag = 1;
	S_X += *Vx/RATE_100_HZ;
	S_Y += *Vy/RATE_100_HZ;

	if(float_abs(Target_S_X - S_X) <= float_abs(*Vx)/RATE_100_HZ||float_abs(*Vx)<0.001)
	{
		*Vx = 0;
		V_X = 0;
		S_X= Target_S_X;
		SX_Flag = 1;
	}
	else 	SX_Flag = 0;
	if(float_abs(Target_S_Y - S_Y) <= float_abs(*Vy)/RATE_100_HZ||float_abs(*Vy)<0.001)
	{
		*Vy = 0;
		V_Y = 0;
		S_Y = Target_S_Y;
		SY_Flag = 1;
	}
	else 	SY_Flag = 0;
	if(SY_Flag && SX_Flag)
		flag_S = 0;
}

/**************************************************************************
函数功能：运动学逆解，根据三轴目标速度计算各车轮目标转速
入口参数：X和Y、Z轴方向的目标运动速度
返回  值：无
**************************************************************************/
void Drive_Motor(float Vx,float Vy,float Vz)
{
		float amplitude=0.3; //Wheel target speed limit //车轮目标速度限幅
	
		if(flag_S) S_Control(&Vx,&Vy);
		else
		{
			S_X += Vx/RATE_100_HZ;
			Target_S_X = S_X;
			S_Y += Vy/RATE_100_HZ;
			Target_S_Y = S_Y; 
		}
		//Inverse kinematics //运动学逆解
		MOTOR_A.Target = +Vy+Vx-Vz*(Wheel_axlespacing+Wheel_spacing);
		MOTOR_B.Target = -Vy+Vx-Vz*(Wheel_axlespacing+Wheel_spacing);
		MOTOR_C.Target = +Vy+Vx+Vz*(Wheel_axlespacing+Wheel_spacing);
		MOTOR_D.Target = -Vy+Vx+Vz*(Wheel_axlespacing+Wheel_spacing);
	
		//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
		MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude); 
		MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
		MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
		MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude);
}
/**************************************************************************
函数功能：FreeRTOS任务，核心运动控制任务
入口参数：无
返回  值：无
**************************************************************************/
void Balance_task(void *pvParameters)
{ 
	  u32 lastWakeTime = getSysTickCnt();
	  float PaiToAng = 57.2957795;
    while(1)
    {	
			//此任务以100Hz的频率运行（10ms控制一次）
			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));

			#if N100
				Angle[2] = AHRSData_Packet.Heading*PaiToAng-180;
				Angle[0] = AHRSData_Packet.Pitch*PaiToAng;
			#endif
			#if Jy901
				Angle[2] = (float)stcAngle.Angle[2]/Angle_factor;
				Angle[0] = (float)stcAngle.Angle[0]/Angle_factor;
			#endif			
			printf("Z:%.3f X:%.3f\r\n",Angle[2],Angle[0]);
			//时间计数，30秒后不再需要
			if(Time_count<3000)Time_count++;
			
			//获取编码器数据，即车轮实时速度，并转换位国际单位
			Get_Velocity_Form_Encoder();

			//系统启动3秒后再进行控制，避免角度传感器启动故障
			if(Time_count>CONTROL_DELAY && Car_Status != 0) 
			{
				V_Z = Vz_Control();
				Drive_Motor(V_X,V_Y,V_Z);
			}
			else
			{
				Drive_Motor(0,0,0);
				Target_Z = Angle[2];
				Angle_X  = Angle[0];
			}			 
			//速度闭环控制计算各电机PWM值，PWM代表车轮实际转速				 
			MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
			MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
			MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
			MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);

			Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm);    //高配麦轮摆式悬挂       SENIOR_MEC_BS MD36N_27
		}	
}
/**************************************************************************
函数功能：赋值给PWM寄存器，控制车轮转速与方向
入口参数：PWM
返回  值：无
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d)
{
	  //Forward and reverse control of motor
	  //电机正反转控制
   	if(motor_a>0)			GPIO_ResetBits(GPIOC,GPIO_Pin_5),	GPIO_SetBits(GPIOB,GPIO_Pin_1);
		else 	            GPIO_SetBits(GPIOC,GPIO_Pin_5),		GPIO_ResetBits(GPIOB,GPIO_Pin_1);  

	  //Forward and reverse control of motor
	  //电机正反转控制	
		if(motor_b>0)			GPIO_ResetBits(GPIOC,GPIO_Pin_4),	GPIO_SetBits(GPIOB,GPIO_Pin_0); 
		else 	            GPIO_SetBits(GPIOC,GPIO_Pin_4),		GPIO_ResetBits(GPIOB,GPIO_Pin_0);  	  

	  //Forward and reverse control of motor
	  //电机正反转控制	
	  if(motor_c>0)			GPIO_ResetBits(GPIOB,GPIO_Pin_5),	GPIO_SetBits(GPIOC,GPIO_Pin_12); 
		else 	            GPIO_SetBits(GPIOB,GPIO_Pin_5),		GPIO_ResetBits(GPIOC,GPIO_Pin_12); 
	  
	  //Forward and reverse control of motor
	  //电机正反转控制	
		if(motor_d>0)			GPIO_ResetBits(GPIOB,GPIO_Pin_4),	GPIO_SetBits(GPIOD,GPIO_Pin_2); 
		else 	            GPIO_SetBits(GPIOB,GPIO_Pin_4),		GPIO_ResetBits(GPIOD,GPIO_Pin_2);  
	  
		//Motor speed control 
	  //电机转速控制
		TIM_SetCompare1(TIM8,myabs(motor_d*DD));
	  //Motor speed control 
	  //电机转速控制
	  TIM_SetCompare4(TIM8,myabs(motor_a*A));  
		//Motor speed control 
	  //电机转速控制
		TIM_SetCompare3(TIM8,myabs(motor_b*B)); 
		//Motor speed control 
	  //电机转速控制
	  TIM_SetCompare2(TIM8,myabs(motor_c*C)); 

}

/**************************************************************************
函数功能：限制PWM值 
入口参数：幅值
返回  值：无
**************************************************************************/
void Limit_Pwm(int amplitude)
{	
	    MOTOR_A.Motor_Pwm=target_limit_float(MOTOR_A.Motor_Pwm,-amplitude,amplitude);
	    MOTOR_B.Motor_Pwm=target_limit_float(MOTOR_B.Motor_Pwm,-amplitude,amplitude);
		  MOTOR_C.Motor_Pwm=target_limit_float(MOTOR_C.Motor_Pwm,-amplitude,amplitude);
	    MOTOR_D.Motor_Pwm=target_limit_float(MOTOR_D.Motor_Pwm,-amplitude,amplitude);
}	    
/**************************************************************************
Function: Limiting function
Input   : Value
Output  : none
函数功能：限幅函数
入口参数：幅值
返回  值：无
**************************************************************************/
float target_limit_float(float insert,float low,float high)
{
    if (insert < low)
        return low;
    else if (insert > high)
        return high;
    else
        return insert;	
}
int target_limit_int(int insert,int low,int high)
{
    if (insert < low)
        return low;
    else if (insert > high)
        return high;
    else
        return insert;	
}
/**************************************************************************
Function: Check the battery voltage, enable switch status, software failure flag status
Input   : Voltage
Output  : Whether control is allowed, 1: not allowed, 0 allowed
函数功能：检查电池电压、使能开关状态、软件失能标志位状态
入口参数：电压
返回  值：是否允许控制，1：不允许，0允许
**************************************************************************/
u8 Turn_Off( int voltage)
{
		u8 temp;
		if(voltage<5||EN==0||Flag_Stop==1)
		{	                                                
			temp=1;      
			PWMA=0;
			PWMB=0;					
		}
		else
			temp=0;
		return temp;						
}
/**************************************************************************
函数功能：求绝对值
入口参数：long int
返回  值：unsigned int
**************************************************************************/
u32 myabs(long int a)
{ 		   
	  u32 temp;
		if(a<0)  temp=-a;  
	  else temp=a;
	  return temp;
}
/**************************************************************************
函数功能：浮点型数据计算绝对值
入口参数：浮点数
返回  值：输入数的绝对值
**************************************************************************/
float float_abs(float insert)
{
	if(insert>=0) return insert;
	else return -insert;
}


/**************************************************************************
函数功能：单击用户按键更新陀螺仪零点
入口参数：无
返回  值：无
**************************************************************************/
void Key(void)
{	
	u8 tmp;
	tmp=click(); 
	if(tmp==1)memcpy(Deviation_gyro,Original_gyro,sizeof(gyro));
}
/**************************************************************************
函数功能：读取编码器数值并计算车轮速度，单位m/s
入口参数：无
返回  值：无
**************************************************************************/
void Get_Velocity_Form_Encoder(void)
{
		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; //用于获取编码器的原始数据 

		//Obtain the original data of the encoder, and the polarity of different models of cars is also different
	  //获取编码器的原始数据，同时不同型号的小车极性也不相同
		#if Mec
			Encoder_A_pr= Read_Encoder(2); 
			Encoder_B_pr= Read_Encoder(3); 
			Encoder_C_pr=-Read_Encoder(4); 
			Encoder_D_pr=-Read_Encoder(5);
		#elif Omni
			Encoder_A_pr=-Read_Encoder(2);
			Encoder_B_pr=-Read_Encoder(3);
			Encoder_C_pr=-Read_Encoder(4);
		#endif

		//The encoder converts the raw data to wheel speed in m/s
		//编码器原始数据转换为车轮速度，单位m/s
		//printf("A:%f B:%f C:%f D:%f\r\n",Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr);
		//printf("%f,%f\r\n",Encoder_A_pr,Encoder_B_pr);
		//printf("%d,%d,%d\r\n",gyro[0],gyro[1],gyro[2]);
		MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
		MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
		MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
		MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
}
/**************************************************************************
函数功能：对三轴目标速度做平滑处理
入口参数：三轴目标速度
返回  值：无
**************************************************************************/
void Smooth_control(float vx,float vy,float vz)
{
	float step=0.4;

	if     (vx>smooth_control.VX) smooth_control.VX+=step;
	else if(vx<smooth_control.VX) smooth_control.VX-=step;
	else                          smooth_control.VX =vx;
	
	if     (vy>smooth_control.VY) smooth_control.VY+=step;
	else if(vy<smooth_control.VY) smooth_control.VY-=step;
	else                          smooth_control.VY =vy;
	
	if     (vz>smooth_control.VZ) smooth_control.VZ+=step;
	else if(vz<smooth_control.VZ) smooth_control.VZ-=step;
	else                          smooth_control.VZ =vz;
	
	if(vx==0&&smooth_control.VX<0.05&&smooth_control.VX>-0.05) smooth_control.VX=0;
	if(vy==0&&smooth_control.VY<0.05&&smooth_control.VY>-0.05) smooth_control.VY=0;
	if(vz==0&&smooth_control.VZ<0.05&&smooth_control.VZ>-0.05) smooth_control.VZ=0;
}
